#include "obstacle_detector/map_processor.h"
#include "cuda_runtime.h"

namespace obstacle_detector
{
//落在静态区域内的点valid_indx设为false
__global__ void remove_static_points_kernel(int16_t *points, int point_num,int static_area_num,
	float *static_areas, bool *valid_index,float ox,float oy,float yaw)
{
	int threadid = blockDim.x * blockIdx.x + threadIdx.x;
	if (threadid>=point_num*static_area_num)
	{
		return;
	}

	int area_id=threadid/point_num;
	int point_id=threadid%point_num;

	float x=points[point_id*4+0]*1.0/256;
	float y=points[point_id*4+1]*1.0/256;

	float new_x=x*cos(-1*yaw)+y*sin(-1*yaw);
	float new_y=-1*x*sin(-1*yaw)+y*cos(-1*yaw);
	new_x+=ox;
	new_y+=oy;

	float bbox_yaw=static_areas[area_id*5+0];
	float center_x=static_areas[area_id*5+1];
	float center_y=static_areas[area_id*5+2];
	float length=static_areas[area_id*5+3];
	float width=static_areas[area_id*5+4];

	new_x-=center_x;
	new_y-=center_y;
	float bbox_x=new_x*cos(bbox_yaw)+new_y*sin(bbox_yaw);
	float bbox_y=-1*new_x*sin(bbox_yaw)+new_y*cos(bbox_yaw);
	// printf ("point: %d,%d,%f,%f\n",point_id,area_id,x,y);
	if (bbox_x<-1*length/2 || bbox_x>length/2 || bbox_y<-1*width/2 || bbox_y>width/2)
	{
		return;
	}

	// printf ("point_id %d,%d\n",point_id,area_id);
	valid_index[point_id]=false;
}

//获取有效点
__global__ void get_valid_points(int16_t *points, uint16_t* points_order,int point_num,
	int16_t *valid_points,uint16_t *valid_points_order,int *valid_points_num,bool *valid_index,
	float x_min,float x_max,float y_min,float y_max)
{
	int threadid = blockDim.x * blockIdx.x + threadIdx.x;
	if (threadid>=point_num)
	{
		return;
	}
	if (!valid_index[threadid])
	{
		return;
	}


	int16_t origin_x=points[threadid*4+0];
	int16_t origin_y=points[threadid*4+1];
	float x=origin_x*1.0/256;
	float y=origin_y*1.0/256;

	if (x<x_min || x>=x_max || y<y_min || y>=y_max)
	{
		return;
	}

	int pointnum_cur=atomicAdd(valid_points_num,1);
	valid_points[pointnum_cur*4+0]=origin_x;
	valid_points[pointnum_cur*4+1]=origin_y;
	valid_points[pointnum_cur*4+2]=points[threadid*4+2];
	valid_points[pointnum_cur*4+3]=points[threadid*4+3];
	valid_points_order[pointnum_cur*2+0]=points_order[threadid*2+0];
	valid_points_order[pointnum_cur*2+1]=points_order[threadid*2+1];
}

__global__ void check_point_road_index(const int16_t *points, int point_num,
	const float *areas, int area_num,uint8_t *point_road_index,float ox,float oy,
	float yaw)
{
	int threadid = blockDim.x * blockIdx.x + threadIdx.x;
	if (threadid>=point_num*area_num)
	{
		return;
	}
	int point_id=threadid%point_num;
	int area_id=threadid/point_num;

	float x=points[point_id*4+0]*1.0/256;
	float y=points[point_id*4+1]*1.0/256;

	float new_x=x*cos(-1*yaw)+y*sin(-1*yaw);
	float new_y=-1*x*sin(-1*yaw)+y*cos(-1*yaw);
	new_x+=ox;
	new_y+=oy;

	float bbox_yaw=areas[area_id*5+0];
	float center_x=areas[area_id*5+1];
	float center_y=areas[area_id*5+2];
	float length=areas[area_id*5+3];
	float width=areas[area_id*5+4];

	new_x-=center_x;
	new_y-=center_y;
	float bbox_x=new_x*cos(bbox_yaw)+new_y*sin(bbox_yaw);
	float bbox_y=-1*new_x*sin(bbox_yaw)+new_y*cos(bbox_yaw);
	if (bbox_x<-1*length/2 || bbox_x>length/2 || bbox_y<-1*width/2 || bbox_y>width/2)
	{
		return;
	}
	point_road_index[point_id]=area_id+1;
}


MapProcessor::MapProcessor(bool debug):
debug_(debug)
{

	cudaMalloc((void **)&static_area_gpu_,sizeof(float)*100*5);
	cudaMalloc((void **)&road_area_gpu_,sizeof(float)*100*5);
	cudaMalloc((void **)&valid_index_gpu_,sizeof(bool)*200000);
	cudaMalloc((void **)&valid_points_gpu_,sizeof(int16_t)*200000*4);
	cudaMalloc((void **)&valid_points_order_gpu_,sizeof(uint16_t)*200000*2);
	cudaMalloc((void **)&valid_points_num_gpu_,sizeof(int));

	if (debug_)
	{
		debug_display_=std::make_unique<DebugDisplay>("map_processor");
	}
}

void MapProcessor::get_roads_yaw(std::vector<float> &roads_yaw)
{
	roads_yaw=roads_yaw_;
}

void MapProcessor::map_process(const module_base::Location &loc,
	int16_t *points, uint16_t *points_order,int &point_num,
	uint8_t *point_road_index,int &roads_num)
{
	// if (debug_)
	// {
	// 	int16_t *points_debug=new int16_t [point_num*4];
	// 	cudaMemcpy(points_debug,points,sizeof(int16_t)*point_num*4,
	// 		cudaMemcpyDeviceToHost);
	// 	float *points_float=new float [point_num*4];
	// 	for (int i=0;i<point_num;i++)
	// 	{
	// 		float x=points_debug[i*4+0]*1.0/256;
	// 		float y=points_debug[i*4+1]*1.0/256;
	// 		float new_x=x*cos(-1*loc.yaw)+y*sin(-1*loc.yaw);
	// 		float new_y=-1*x*sin(-1*loc.yaw)+y*cos(-1*loc.yaw);
	// 		points_float[i*4+0]=new_x+loc.x;
	// 		points_float[i*4+1]=new_y+loc.y;
	// 		points_float[i*4+2]=points_debug[i*4+2]*1.0/256;

	// 	}
	// 	debug_display_->set_showinfo_float(points_float,point_num);
	// 	delete [] points_debug;
	// 	delete [] points_float;
	// }


	std::vector<float> area_data;
	std::vector<Eigen::MatrixXf> vertexs;
	get_related_area(loc,all_static_areas_,area_data,vertexs,70);
	// get_related_fence(loc,all_fence_areas_,area_data,vertexs,70);
	std::cout<<"area_data "<<area_data.size()<<std::endl;
	int area_num=area_data.size()/5;
	printf ("area_num: %d\n",area_num);
	if (area_num==0 || point_num==0)
	{
		printf("area_num==0 || point_num==0 \n");
		return;
	}
	if (debug_)
	{
		debug_display_->set_static_area(vertexs);
	}
	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
	cudaMemcpy(static_area_gpu_,area_data.data(),sizeof(float)*area_num*5,
		cudaMemcpyHostToDevice);
	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
	cudaMemset(valid_index_gpu_,1,sizeof(bool)*point_num);
	cudaMemset(valid_points_num_gpu_,0,sizeof(int));

	int grid_num1=(point_num*area_num+1024-1)/1024;
	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
	remove_static_points_kernel<<<grid_num1,1024>>>(points,point_num,area_num,
		static_area_gpu_,valid_index_gpu_,loc.x,loc.y,loc.yaw);

	int grid_num2=(point_num+1024-1)/1024;
	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
	get_valid_points<<<grid_num2,1024>>>(points,points_order,point_num,
		valid_points_gpu_,valid_points_order_gpu_,valid_points_num_gpu_,valid_index_gpu_,
		min_x_,max_x_,min_y_,max_y_);

	int valid_points_num;
	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
	cudaMemcpy(&valid_points_num,valid_points_num_gpu_,sizeof(int),
		cudaMemcpyDeviceToHost);

	printf ("valid_points_num: %d\n",valid_points_num);
	point_num=valid_points_num;
	if (valid_points_num==0)
	{
		printf("valid_points_num == 0 return ");
		return;
	}


	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
	check_point_road(loc,point_road_index,roads_num,valid_points_num);

	if (debug_)
	{
		int16_t *points_debug=new int16_t [valid_points_num*4];
		cudaMemcpy(points_debug,valid_points_gpu_,sizeof(int16_t)*valid_points_num*4,
			cudaMemcpyDeviceToHost);
		float *points_float=new float [valid_points_num*4];
		for (int i=0;i<valid_points_num;i++)
		{
			float x=points_debug[i*4+0]*1.0/256;
			float y=points_debug[i*4+1]*1.0/256;
			float new_x=x*cos(-1*loc.yaw)+y*sin(-1*loc.yaw);
			float new_y=-1*x*sin(-1*loc.yaw)+y*cos(-1*loc.yaw);
			points_float[i*4+0]=new_x+loc.x;
			points_float[i*4+1]=new_y+loc.y;
			points_float[i*4+2]=points_debug[i*4+2]*1.0/256;

		}
		debug_display_->set_showinfo_float(points_float,valid_points_num);
		delete [] points_debug;
		delete [] points_float;
	}
	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
	cudaMemcpy(points,valid_points_gpu_,sizeof(int16_t)*point_num*4,
		cudaMemcpyDeviceToDevice);
	cudaMemcpy(points_order,valid_points_order_gpu_,sizeof(uint16_t)*point_num*4,
		cudaMemcpyDeviceToDevice);

	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
}

void MapProcessor::check_point_road(const module_base::Location &loc,
	uint8_t *point_road_index,int &roads_num,int point_num)
{
	std::vector<float> area_data;
	std::vector<Eigen::MatrixXf> vertexs;
	printf("all_road_areas_ \n");
	std::cout<<"all_road_areas_ "<<all_road_areas_.size()<<std::endl;
	get_related_area(loc,all_road_areas_,area_data,vertexs,50);
	std::cout<<"area_data "<<area_data.size()<<std::endl;
	roads_num=area_data.size()/5;
	roads_yaw_.clear();
	roads_yaw_.resize(roads_num);
	for (int i=0;i<roads_num;i++)
	{
		roads_yaw_[i]=area_data[i*5];
	}
	if (roads_num==0) return;
	if (debug_)
	{
		debug_display_->set_road_area(vertexs);
	}
	cudaMemcpy(road_area_gpu_,area_data.data(),sizeof(float)*roads_num*5,
		cudaMemcpyHostToDevice);
	cudaMemset(point_road_index,0,sizeof(uint8_t)*point_num);
	int grid_num=(roads_num*point_num+1024-1)/1024;

	check_point_road_index<<<grid_num,1024>>>(valid_points_gpu_,point_num,
		road_area_gpu_,roads_num,point_road_index,loc.x,loc.y,loc.yaw);

	// cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
}

//获取当前区域附近的静态区域
void MapProcessor::get_related_area(const module_base::Location &loc,
	const std::vector<Region> all_areas,std::vector<float> &area_data,
	std::vector<Eigen::MatrixXf> &vertexs, float threshold)
{
	for (int i=0;i<all_areas.size();i++)
	{
		float diff_x=loc.x - all_areas[i].center_x;
		float diff_y=loc.y - all_areas[i].center_y;
		float yaw=all_areas[i].yaw;
		float dis_x=fabs(diff_x*cos(yaw)+diff_y*sin(yaw));
		float dis_y=fabs(-1*diff_x*sin(yaw)+diff_y*cos(yaw));
		if (dis_x<(all_areas[i].length/2+threshold) && dis_y< (all_areas[i].width/2+threshold))
		{
			area_data.push_back(all_areas[i].yaw);
			area_data.push_back(all_areas[i].center_x);
			area_data.push_back(all_areas[i].center_y);
			area_data.push_back(all_areas[i].length);
			area_data.push_back(all_areas[i].width);
			// printf ("static_area id: %d\n",all_areas[i].id);
			if (debug_)
			{
				vertexs.push_back(all_areas[i].vertexs);
			}
			// break;
		}
	}
	printf ("***************\n");
}

// //获取当前区域附近的静态区域中的栅栏
// void MapProcessor::get_related_fence(const module_base::Location &loc,
// 	const std::vector<Region> all_areas,std::vector<float> &area_data,
// 	std::vector<Eigen::MatrixXf> &vertexs, float threshold)
// {

// 	// std::cout<<"loc.x "<<loc.x<<" "<<"loc.y "<<loc.y<<std::endl;
// 	for (int i=0;i<all_areas.size();i++)
// 	{
// 		float diff_x=loc.x - all_areas[i].center_x;
// 		float diff_y=loc.y - all_areas[i].center_y;
// 		float yaw=all_areas[i].yaw;
// 		float dis_x=fabs(diff_x*cos(yaw)+diff_y*sin(yaw));
// 		float dis_y=fabs(-1*diff_x*sin(yaw)+diff_y*cos(yaw));
// 		if (dis_x<(all_areas[i].length/2+threshold) && dis_y<(all_areas[i].width/2+threshold))
// 		{
// 			float fence_area_extend=100;//延长100m
// 			float area_extend_width=fence_area_extend;
// 			float area_extend_length=fence_area_extend;
// 			//先判断区域短边的朝向 假设fance的法向为短边朝向
// 			//区域yaw默认为length边的方向
// 			//延长区域短边 缩短区域长边 
// 			float short_side_yaw=yaw;
// 			if(all_areas[i].length>all_areas[i].width)
// 			{
// 				short_side_yaw=yaw+M_PI/2;//如果不是短边朝向 就+90度
// 				area_extend_width=fence_area_extend;
// 				area_extend_length=0.0;
// 				if(is_in_extended_area(loc,all_areas[i],0.0,70.0)==0)
// 				{
// 					continue;
// 				}
// 			}
// 			else
// 			{
// 				area_extend_width=0.0;
// 				area_extend_length=fence_area_extend;
// 				if(is_in_extended_area(loc,all_areas[i],70.0,0.0)==0)
// 				{
// 					continue;
// 				}
// 			}

// 			// std::cout<<"all_areas[i].center_x "<<all_areas[i].center_x<<" "<<"all_areas[i].center_y "<<all_areas[i].center_y<<std::endl;
// 			// std::cout<<"diff_x "<<diff_x<<" "<<"diff_y "<<diff_y<<std::endl;
// 			// std::cout<<"short_side_yaw "<<short_side_yaw*180/M_PI<<" "<<cos(short_side_yaw)<<" "<<sin(short_side_yaw)<<std::endl;
// 			if(diff_x*cos(short_side_yaw)+diff_y*sin(short_side_yaw) > 0){
// 				short_side_yaw=short_side_yaw+M_PI;
// 			}
// 			area_data.push_back(all_areas[i].yaw);
// 			area_data.push_back(all_areas[i].center_x+fence_area_extend/2*cos(short_side_yaw));
// 			area_data.push_back(all_areas[i].center_y+fence_area_extend/2*sin(short_side_yaw));
// 			area_data.push_back(all_areas[i].length+area_extend_length);
// 			area_data.push_back(all_areas[i].width+area_extend_width);
// 			// printf ("static_area id: %d\n",all_areas[i].id);
// 			if (debug_)
// 			{
// 					float area_length_=(all_areas[i].length+area_extend_length)/2;
// 					float area_width_ =(all_areas[i].width +area_extend_width) /2;
// 					Eigen::MatrixXf area_4point(3,4);
// 					area_4point<<	+area_length_,+area_length_,-area_length_,-area_length_,
// 									+area_width_ ,-area_width_ ,-area_width_ ,+area_width_ ,
// 									            1,            1,            1,            1;
// 					Eigen::MatrixXf tarns(3,3);
// 					tarns<<cos(-yaw),sin(-yaw),all_areas[i].center_x+fence_area_extend/2*cos(short_side_yaw),
// 					  (-1)*sin(-yaw),cos(-yaw),all_areas[i].center_y+fence_area_extend/2*sin(short_side_yaw),
// 						           0,        0,                                                            1;
// 					area_4point=tarns*area_4point;
// 					Eigen::MatrixXf area_debug(2,4);
// 					area_debug.block<2,4>(0,0)=area_4point.block<2,4>(0,0);
// 					vertexs.push_back(area_debug);
// 			}
// 		}
// 	}
// 	// printf ("***************\n");
// }

void MapProcessor::set_map(std::string map_path)
{
	size_t pos = map_path.find(".");
	std::string new_path = map_path.substr(0, pos)+"_structure.osm";
	lanelet::projection::UtmProjector projector(lanelet::Origin({49, 8.4}));
	map_ = lanelet::load(new_path, projector);
	printf ("lanelet num: %ld\n",map_->laneletLayer.size());
	all_road_areas_.clear();
	all_fence_areas_.clear();
	all_static_areas_.clear();

	lanelet::traffic_rules::TrafficRulesPtr traffic_rules =
		lanelet::traffic_rules::TrafficRulesFactory::create(lanelet::Locations::Germany, lanelet::Participants::Vehicle);
	lanelet::routing::RoutingGraphUPtr routing_graph = lanelet::routing::RoutingGraph::build(*map_, *traffic_rules);
	float count_layer=0;
	for (auto it=map_->laneletLayer.begin();it!=map_->laneletLayer.end();it++)
	{
		lanelet::Optional<bool> is_straight = it->attributes()["is_straight"].asBool();
		if (!is_straight || !(*is_straight))
		{
			continue;
		}
		auto previous_lanelets=routing_graph->previous(*it);
		bool is_begin=true;
		for (int i=0;i<previous_lanelets.size();i++)
		{
			lanelet::Optional<bool> is_straight = map_->laneletLayer.get(previous_lanelets[i].id()).attributes()["is_straight"].asBool();
			if (is_straight && *is_straight)
			{
				is_begin=false;
				break;
			}
		}
		if (!is_begin)
		{
			continue;
		}

		// printf ("is_begin: %d\n",it->id());
		Eigen::MatrixXf area_vertexs(2,4);
		area_vertexs(0,0)=it->leftBound()[0].x();
		area_vertexs(1,0)=it->leftBound()[0].y();
		area_vertexs(0,3)=it->rightBound()[0].x();
		area_vertexs(1,3)=it->rightBound()[0].y();
		lanelet::ConstLanelet current_road=*it;
		while (true)
		{

			auto following_lanelets=routing_graph->following(current_road);
			bool is_end=true;
			for (int i=0;i<following_lanelets.size();i++)
			{
				lanelet::Optional<bool> is_straight = map_->laneletLayer.get(following_lanelets[i].id()).attributes()["is_straight"].asBool();
				if (is_straight && *is_straight)
				{
					is_end=false;
					current_road=following_lanelets[i];
					break;
				}
			}
			if (is_end)
			{
				break;
			}
		}
		int left_size=current_road.leftBound().size()-1;
		int right_size=current_road.rightBound().size()-1;
		area_vertexs(0,1)=current_road.leftBound()[left_size].x();
		area_vertexs(1,1)=current_road.leftBound()[left_size].y();
		area_vertexs(0,2)=current_road.rightBound()[right_size].x();
		area_vertexs(1,2)=current_road.rightBound()[right_size].y();
		// printf ("vertexs size: %d,%d\n",area_vertexs.rows(),area_vertexs.cols());

		Region road_region;
		road_region.id=it->id();
		road_region.set_info(area_vertexs);
		all_road_areas_.push_back(road_region);
		count_layer++;
	}
	printf("count_layer : %f \n",count_layer);


	for (auto it=map_->polygonLayer.begin();it!=map_->polygonLayer.end();it++)
	{
		// std::cout<<it->id()<<" "<<it->attributes()["eliminate"]<<std::endl;
		lanelet::Optional<bool> eliminate_map = it->attributes()["eliminate"].asBool();
		if (eliminate_map && *eliminate_map)
		{
			// std::cout<<it->id()<<" "<<it->attributes()["eliminate"]<<std::endl;
			auto polygon_area=map_->polygonLayer.get(it->id());
			Eigen::MatrixXf area_vertexs(2,4);
			for (int i=0;i<polygon_area.size();i++)
			{
				area_vertexs(0,i)=polygon_area[i].x();
				area_vertexs(1,i)=polygon_area[i].y();		
			}

			Region static_area;
			static_area.id=it->id();
			static_area.set_info(area_vertexs);
			all_static_areas_.push_back(static_area);

			lanelet::Optional<bool> fence_map = it->attributes()["fence"].asBool();
			if (fence_map && *fence_map)
			{
				all_fence_areas_.push_back(static_area);
			}
		}
	}
}

}
